#include <iostream>
#include <cstdlib>

#include "ABAData.h"
#include "MatrixClasses.h"
#include "Joint.h"
#include "Transform.h"
#include "FastOps.h"
#include "EOMData.h"


using namespace RSIM;

///////////////////////////////////////////////////////

ABAData::ABAData():Ik3_(NULL),Fk3_(NULL),cFk_(NULL){
}

///////////////////////////////////////////////////////

ABAData::ABAData(const ABAData& AB):Ik3_(NULL),Fk3_(NULL),cFk_(NULL){
}

///////////////////////////////////////////////////////

ABAData::~ABAData(){
        delete cFk_;	
        delete [] Ik3_;
        delete [] Fk3_;
}

///////////////////////////////////////////////////////

void ABAData::clear(){
        delete cFk_;		cFk_ = NULL;	
        delete [] Ik3_; 	Ik3_ = NULL;
        delete [] Fk3_;		Fk3_ = NULL;
}

///////////////////////////////////////////////////////

void ABAData::initForwardDynamics(const Joint *J){
        const int& dof = J->getJointDOF();
        
        if(!J->getIsMotionPrescribedAndActive()){		
                if(dof){			
                        this->Ik3_ = new Mat6x6 [dof];
                        this->Fk3_ = new Vect6 [dof];
                        this->cFk_ = new Vect6;	// sFlag
                }
                else{ // for body with rigid joint
                        this->Ik3_ = new Mat6x6[1];
                        this->Fk3_ = new Vect6[1];
                }
        }
        else{
                this->cFk_ = new Vect6;	
                this->Ik3_ = new Mat6x6[1];
                this->Fk3_ = new Vect6[1];
        }
}

///////////////////////////////////////////////////////